import time
import cv2
import RPi.GPIO as GPIO
from servo import Servo

# GPIO引脚初始化
servo1_pin = 19  # 下舵机
servo2_pin = 18  # 上舵机
GPIO.setmode(GPIO.BCM)  # BCM引脚编号模式
GPIO.setwarnings(False)
servo1 = Servo(servo1_pin)
servo2 = Servo(servo2_pin, is_restricted=True)
MAX_ERR = 20

cap = cv2.VideoCapture(0)
fwidth = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH) / 3)
fheight = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT) / 3)  # 获取帧大小
mX, mY = fwidth / 2, fheight / 2
# print(fwidth, fheight)

def servo_rotate(servo, angle=3):  # angle: 舵机转动的角度（-10~10）
    if angle != 0:
        servo.set_angle(servo.angle + angle)
        time.sleep(0.05)  # 50ms

while cap.isOpened():
    ret, frame = cap.read()
    frame = cv2.resize(frame, (fwidth, fheight))
    time1 = time.time()
    face_detector = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    # 检测人脸，返回人脸的位置信息
    faces = face_detector.detectMultiScale(frame)
    if len(faces) == 1:  # detect one face
        for x, y, w, h in faces:
            xx, yy = x + w / 2, y + h / 2  # 获取矩形中点
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
            if xx - mX > MAX_ERR:
                servo1.rotate(-2)
            elif mX - xx > MAX_ERR:
                servo1.rotate(2)
            if yy - mY > MAX_ERR:
                servo2.rotate(-2)
            elif mY - yy > MAX_ERR:
                servo2.rotate(2)
            print(f'cur=({xx},{yy}),target=({mX},{mY})')
    else:
        servo1.__set_cur_pwm__()
        servo2.__set_cur_pwm__()
    # 显示图像
    time2 = time.time()
    cv2.putText(frame, f'fps={int(1 / (time2 - time1))}', (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
    cv2.imshow('video', frame)
    key = cv2.waitKey(1)
    if key == 27:
        break

cap.release()
cv2.destroyAllWindows()
GPIO.cleanup()
